/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvWorld/Src/world.cpp
 * @Description  : 实现了World类，提供了与地图和姿态相关的数据提取和处理功能。
 *                 包括从路径中提取项目数据、获取节点姿态、加载地图文件等方法。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:30:07
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "world.h"
#include <fstream> 

//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "CWorld".
//   Calculate the deviation according to the pose
//
//   Get the project data of a specified posture to a specified path.
//
/**
 * @brief 从路径中提取项目数据
 *
 * 从指定的路径中提取项目数据，并将结果存储在提供的Posture和ProjectData对象中。
 *
 * @param posture 引用传递的Posture对象，用于存储提取的姿态数据
 * @param path 指向Path对象的指针，表示要从中提取数据的路径。如果为nullptr，则输出错误信息并返回false。
 * @param project_data 指向ProjectData对象的指针，用于存储提取的项目数据
 *
 * @return 如果成功提取数据，则返回true；否则返回false。
 */
bool World::GetProjectData(Posture& posture, Path* path, ProjectData* project_data) 
{
  bool result = false;
  try {
    if (path == nullptr) {
      DEBUG_ERROR_OUT("GetProjectData: path is null");
    }

    Trajectory* traj = path->MakeTraj();
    if (traj == nullptr) {
      DEBUG_ERROR_OUT("GetProjectData: traj is null");
    }

    bool bezier_init_posture = traj->GetType() == (short)TrajType::BEZIER_TRAJ;
    result = traj->ProjectPosture(posture, project_data, bezier_init_posture);
    delete traj;
  } catch (const std::exception& e) {
    DEBUG_ERROR_OUT("Exception in GetProjectData: " << e.what());
  }
  return result;
}

//   Given a pose, traverse the map to find the closest link and deviation
//
//   Get the project data of a specified posture to the map.
//
/**
 * @brief 从项目中获取姿态相关的数据
 *
 * 从项目中获取与给定姿态相关的数据，并存储在提供的ProjectData指针中。
 *
 * @param posture 给定的姿态
 * @param project_data 用于存储结果的ProjectData指针
 *
 * @return 如果成功获取到数据，则返回true；否则返回false
 */
bool World::GetProjectData(Posture& posture, ProjectData* project_data) 
{
  unsigned short u = 0xFFFF;
  ProjectData result_data, temp_data;
  bool result = false;
  try {
    for (unsigned short i = 0; i < PathBase::path_count_; i++) {
      if (GetProjectData(posture, path_index_[i].ptr_, &temp_data)) {
        if (u == 0xFFFF || temp_data < result_data) {
          result_data = temp_data;
          u = i;
          result = true;
          *project_data = result_data;
        }
      }
    }
  } catch (const std::exception& e) {
    DEBUG_ERROR_OUT("Exception in GetProjectData: " << e.what());
  }
  return result;
}

/**
 * @brief 获取指定节点在指定方向上的姿态
 *
 * 根据提供的起始节点和方向节点，获取从起始节点到方向节点的路径和姿态信息。
 *
 * @param at_node 起始节点编号
 * @param dir_node 方向节点编号
 * @param posture 用于存储姿态信息的结构体
 * @return 如果成功获取姿态信息，则返回true；否则返回false
 */
bool World::GetNodePosture(unsigned short at_node, unsigned short dir_node, Posture& posture) 
{
  try {
    if (at_node == dir_node || !IsNode(at_node) || !IsNode(dir_node)) {
      return false;
    }

    Path* path = GetPathPointer(at_node, dir_node);
    Node* node = GetNode(at_node);

    if (!path || !node) {
      return false;
    }

    posture.theta_ = path->GetHeading(*node).radian_;
    posture.x_ = node->x_;
    posture.y_ = node->y_;
  } catch (const std::exception& e) {
    DEBUG_ERROR_OUT("Exception in GetNodePosture: " << e.what() << " at_node=" << at_node << ", dir_node=" << dir_node);
  }
  return true;
}

/**
 * @brief 加载地图文件
 *
 * 从指定路径加载地图文件，并初始化地图的节点和路径信息。
 *
 * @param map_path 地图文件路径
 * @return 如果成功加载地图，则返回true；否则返回false
 */
bool World::LoadMap(const std::string& map_path) 
{
  try {
    NodeBase& node_base = GetNodeBaseObject();
    Path::node_base_ = &node_base;
    bool read_node = node_base.Create(map_path);

    PathBase& path_base = *GetPathBaseObject();
    bool read_path = path_base.Create(map_path);

    return read_node && read_path;
  } catch (const std::exception& e) {
    DEBUG_ERROR_OUT("Exception in LoadMap: " << e.what());
  }
  return false;
}

/**
 * @brief 获取世界地图的ID
 *
 * 返回当前世界的地图ID。
 *
 * @return std::string 当前世界的地图ID
 */
std::string World::GetMapId() const 
{
  return map_id_;
}